# coding:utf-8
#!/usr/bin/python
     
# Extract images from a bag file.
import roslib; #roslib.load_manifest(PKG)
import rosbag
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from cv_bridge import CvBridgeError
     
#Reading bag filename from command line or roslaunch parameter.
#import os
#import sys
def string_to_float(str):
	return float(str)
     
cam0_path  = '/home/jankin/linux/python/cam/'     # 已经建立好的存储cam0 文件的目录
cam1_path  = '/home/jankin/linux/python/cam/'
filesave = open('pictime.txt', "w")
GpsSave = open('gpsfile.txt', "w")
ImuSave = open('imufile.txt', "w")
OdomSave = open('odomfile.txt', "w")
timestr_last=0.0
last_lat=0.0
GpsTime=0.0
GpsLatStd=0.0
ImuTime=0.0
OdomTime=0.0
class ImageCreator():
    def __init__(self):
        self.bridge = CvBridge()
        with rosbag.Bag('/home/jankin/data/bag_data/2023-03-10-02-09-53.bag', 'r') as bag:  #要读取的bag文件；
            for topic,msg,t in bag.read_messages():
                if topic == "/camera/rgb/image_raw": #图像的topic；
                        try:
                            cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8")
                        except CvBridgeError as e:
                            print (e)
                        timestr = "%.6f" %  msg.header.stamp.to_sec()
                            #%.6f表示小数点后带有6位，可根据精确度需要修改；
                        image_name = timestr+ ".jpg" #图像命名：时间戳.jpg
                        #cv2.imwrite(cam0_path + image_name, cv_image)  #保存；
			global timestr_last
			filesave.write(str(timestr)+' ')
			if float(timestr)-float(timestr_last)>0.11 or float(timestr)-float(timestr_last)<0.09 :
                        	filesave.write(str(float(timestr)-float(timestr_last)))
			filesave.write('\n')
			timestr_last=timestr
						
                elif topic == "/GPS_fix": #图像的topic；
                        timegps = "%.6f" %  msg.header.stamp.to_sec()#%.6f表示小数点后带有6位，可根据精确度需要修改；
			gps_lat=msg.latitude
			gps_lat_std=msg.position_covariance[0]
			global last_lat
			global GpsTime
			global GpsLatStd
			if last_lat!=gps_lat or gps_lat_std!=GpsLatStd :
				GpsSave.write(str(timegps)+' '+str(gps_lat)+' ')
				if  float(timegps)-float(GpsTime)>0.22 or float(timegps)-float(GpsTime)<0.18:
					GpsSave.write(str(float(timegps)-float(GpsTime)))
				GpsSave.write('\n')
				GpsTime=timegps
			last_lat=gps_lat
			GpsLatStd=gps_lat_std

		elif topic == "/Ser_imu":
			TimeImu="%.6f" %  msg.header.stamp.to_sec()
			global ImuTime
			if float(TimeImu)-float(ImuTime)>0.012 or float(TimeImu)-float(ImuTime)<0.008:
				ImuSave.write(str(float(TimeImu)-float(ImuTime))+'\n')
			ImuTime=TimeImu
			

		elif topic == "/Ser_odom":
     			TimeOdom="%.6f" %  msg.header.stamp.to_sec()
			global OdomTime
			if float(TimeOdom)-float(OdomTime)>0.012 or float(TimeOdom)-float(OdomTime)<0.008:
				OdomSave.write(str(float(TimeOdom)-float(OdomTime))+'\n')
			OdomTime=TimeOdom

if __name__ == '__main__':
        #rospy.init_node(PKG)
    try:
        image_creator = ImageCreator()
    except rospy.ROSInterruptException:
        pass
    filesave.close()
    GpsSave.close()
    ImuSave.close()
    OdomSave.close()



